from sympy import symbols, Matrix

# 定义符号变量
# x1 = q_1, x2 = pi+q_2, x3 = -pi/2 + q_3, x4 = pi + q_4, x5 = pi/2 + q_5, x6 = q_6
c_x1, s_x1, c_x2, s_x2, c_x3, s_x3, c_x4, s_x4, c_x5, s_x5, c_x6, s_x6, l = symbols('c_x1 s_x1 c_x2 s_x2 c_x3 s_x3 c_x4 s_x4 c_x5 s_x5 c_x6 s_x6 l')
T01 = Matrix([[c_x1, -s_x1, 0, 0], [s_x1, c_x1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
T12 = Matrix([[c_x2, -s_x2, 0, 0], [0, 0, 1, 0], [-s_x2, -c_x2, 0, 0], [0, 0, 0, 1]])
T23 = Matrix([[c_x3, -s_x3, 0, 0], [0, 0, 1, 0.5], [-s_x3, -c_x3, 0, 0], [0, 0, 0, 1]])
T34 = Matrix([[c_x4, -s_x4, 0, 0], [0, 0, 1, 0], [-s_x4, -c_x4, 0, 0], [0, 0, 0, 1]])
T45 = Matrix([[c_x5, -s_x5, 0, 0], [0, 0, 1, 0.5], [-s_x5, -c_x5, 0, 0], [0, 0, 0, 1]])
T56 = Matrix([[c_x6, 0, -s_x6, 0], [0, 1, 0, -l], [s_x6, 0, c_x6, 0], [0, 0, 0, 1]])





# 符号矩阵乘法
T06 = T01*T12*T23*T34*T45*T56

# 输出结果
print("矩阵乘积 T06 = T01*T12*T23*T34*T45*T56 \n")
print(T06)